/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include <stdint.h>
#include "motor.h"
#include "rtthread.h"
#define MOTOR_COUNT 2
#define MAX_THROTTLE 1000
#define MAX_ACCELERATION 25
enum vehicleState
{
    VEHICLE_IDLE = 1,
    VEHICLE_STOP = 1 << 1,
    VEHICLE_RUN = 1 << 2,
    VEHICLE_ACCELERATE = 1 << 3,
    VEHICLE_DECELERATE = 1 << 4,

};
struct vehicleCtrl
{
    int16_t motor[MOTOR_COUNT];
    // int16_t speed[MOTOR_COUNT];
    int16_t throttle;
    int16_t turn;
    uint16_t state;
    uint16_t preState;

} vehicle_ctrl = {0};
void vehicle_init(void)
{
}
void vehicle_left(void)
{
    vehicle_ctrl.turn = -MAX_THROTTLE;
}
void vehicle_right(void)
{
    vehicle_ctrl.turn = MAX_THROTTLE;
}
void vehicle_start(void)
{
    vehicle_ctrl.state |= VEHICLE_RUN;
    rt_kprintf("vehicle_start\n");
}
void vehicle_stop(void)
{
    vehicle_ctrl.state &= ~VEHICLE_RUN;
    rt_kprintf("vehicle_stop\n");
}
void vehicle_break(void)
{
    vehicle_ctrl.turn = 0;
    vehicle_ctrl.throttle = 0;
    rt_kprintf("vehicle_break\n");
}
void vehicle_forward(void)
{
    // int16_t acc = MAX_THROTTLE - vehicle_ctrl.throttle;
    // vehicle_ctrl.throttle += ((acc > MAX_ACCELERATION) ? MAX_ACCELERATION : acc);
    // if (vehicle_ctrl.throttle > MAX_THROTTLE)
    // {
    //     vehicle_ctrl.throttle = MAX_THROTTLE;
    // }
    // rt_kprintf("vehicle_ctrl.throttle %d\n",vehicle_ctrl.throttle);
    vehicle_ctrl.throttle = MAX_THROTTLE;
}
void vehicle_backward(void)
{
    // int16_t acc = MAX_THROTTLE + vehicle_ctrl.throttle;
    // vehicle_ctrl.throttle -= ((acc > MAX_ACCELERATION) ? MAX_ACCELERATION : acc);
    // if (vehicle_ctrl.throttle < -MAX_THROTTLE)
    // {
    //     vehicle_ctrl.throttle = -MAX_THROTTLE;
    // }
    vehicle_ctrl.throttle = -MAX_THROTTLE;
}
void vehicle_run(void)
{
    if (vehicle_ctrl.state & VEHICLE_RUN)
    {
        int16_t m1 = vehicle_ctrl.throttle + vehicle_ctrl.turn;
        int16_t m2 = vehicle_ctrl.throttle - vehicle_ctrl.turn;

        vehicle_ctrl.motor[0] += ((m1 - vehicle_ctrl.motor[0]) * 0.01f); // lpf
        vehicle_ctrl.motor[1] += ((m2 - vehicle_ctrl.motor[1]) * 0.01f);

        motor1_set_duty(vehicle_ctrl.motor[0]);
        motor2_set_duty(vehicle_ctrl.motor[1]);
        // rt_kprintf("motor: %4d %4d, thr:%4d turn:%4d\n", vehicle_ctrl.motor[0], vehicle_ctrl.motor[1], vehicle_ctrl.throttle, vehicle_ctrl.turn);
    }
    if (vehicle_ctrl.preState != vehicle_ctrl.state)
    {
        rt_kprintf("vehicle state change %d->%d\n", vehicle_ctrl.preState, vehicle_ctrl.state);
        if (!(vehicle_ctrl.state & VEHICLE_RUN))
        {
            if (vehicle_ctrl.preState & VEHICLE_RUN)
            {
                vehicle_ctrl.motor[0] = 0;
                vehicle_ctrl.motor[1] = 0;
                vehicle_ctrl.throttle = 0;
                vehicle_ctrl.turn = 0;
                motor1_set_duty(0);
                motor2_set_duty(0);
            }
        }
        vehicle_ctrl.preState = vehicle_ctrl.state;
    }
}
